// Writen by Alex Wilson
// Modded Heavily by Cj
// for the Caltech Rover Team
// Created: May 2012
// Last Updated : May 12, 2012

// Gets potentiomenter input from the control arm setup and transmits the data over serial.

//Pan, sholder, elbow, wrist, twist, grab

#include <config.h>

String outputString = "";
String temp = "";


int PanInputPin = 0; //the input pin for the elbow potentiometer
int PanInputValue = 0; //the variable to store the elbow's value

int ShoulderInputPin = 1; //the input pin for the shoulder potentiometer
int ShoulderInputValue = 0; //the variable to store the shoulder's value

int ElbowInputPin = 2; //the input pin for the elbow potentiometer
int ElbowInputValue = 0; //the variable to store the elbow's value

int WristInputPin = 3; //the input pin for the wrist potentiometer
int WristInputValue = 0; //the variable to store the wrist's value

int TwistInputPin = 4; //the input pin for the twist potentiometer
int TwistInputValue = 0; //the variable to store the twist's value

int GrabberInputPin = 5; //the input pin for the grabber potentiometer
int GrabberInputValue = 0; //the variable to store the grabber's value

int ClawInputPin = 46;
int ClawInputValue = 0;

int EnableArmPin = 52;
int EnableArmValue = 0;

void setup(){
  Serial.begin(115200);
  //  Serial.println("Potentiometers online");
  outputString.reserve(200);
  temp.reserve(200);
  pinMode(ClawInputPin, INPUT);
  pinMode(EnableArmPin, INPUT);
//  delay(2000);
}


void addToString(int value, char next){
  int returnValue = value * 255./1024;
//  Serial.println(returnValue);
  temp += returnValue;
  temp += next;
}

void send_to_comp(String inputString){
  String output = "";
  int length = inputString.length();
  
  char buff[inputString.length() + 1];
  inputString.toCharArray(buff, inputString.length() + 1);
  
  // Assume correct delinate
  for(int i = 0; i < inputString.length(); ++i){
    if(i % 2 == 0){
      output += inputString.charAt(i);
    }else{
      unsigned char c = inputString.charAt(i);
      int intForm = (int)c;
      output += intForm;
    }
  }

  Serial.println(output);
}

void loop() //loop infinitely reading the analog values
{
  temp = "";
  // Add first Delinators
  temp += STRING_STAR_POINT;

  PanInputValue = analogRead(PanInputPin); //get the pan value and send it over serial
  addToString(PanInputValue, STRING_DELINATOR);
  //  Serial.print("Pan value: ");
  //  Serial.println(PanInputValue);
  
  ShoulderInputValue = analogRead(ShoulderInputPin); //get the shoulder value and send it over serial
  addToString(ShoulderInputValue, STRING_DELINATOR);
  //  Serial.print("Shoulder value: ");
  //  Serial.println(ShoulderInputValue);
  
  ElbowInputValue = analogRead(ElbowInputPin); //get the elbow value and send it over serial
  addToString(ElbowInputValue, STRING_DELINATOR);
  //  Serial.print("Elbow value: ");
  //  Serial.println(ElbowInputValue);
  
  WristInputValue = analogRead(WristInputPin); //get the wrist value and send it over serial
  addToString(WristInputValue, STRING_DELINATOR);
  //  Serial.print("Wrist value: ");
  //  Serial.println(WristInputValue);
  
  TwistInputValue = analogRead(TwistInputPin); //get the twist value and send it over serial
  addToString(TwistInputValue, STRING_DELINATOR);
  //  Serial.print("Twist value: ");
  //  Serial.println(TwistInputValue);
  
  GrabberInputValue = analogRead(GrabberInputPin); //get the grabber value and send it over serial
  addToString(GrabberInputValue, STRING_DELINATOR);
  //  Serial.print("Grabber value: ");
  //  Serial.println(GrabberInputValue);

  ClawInputValue = digitalRead(ClawInputPin);
  EnableArmValue = digitalRead(EnableArmPin);

  // Arduino hates 0s
  int buttons = 1;
  if(ClawInputValue == 1){
  	buttons += 2;
  }
  if(EnableArmValue == 1){
        buttons += 4;
  }
  temp += buttons;
  temp += STRING_END_POINT;

  //  Serial.println(); //merely for asthetics to distinguish between reads
  outputString = temp;
  
  delay(33); // wait a little bit so you are reading at a reasonable rate
}

void sendStringtoComp(void){
  Serial.println(outputString);
}

void serialEvent(){
  while(Serial.available()){
    char inChar = (char)Serial.read();
    if (inChar == '#'){
      sendStringtoComp();
      outputString = "";
    }
  }
}
